#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <stdio.h>
#include <stdlib.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <string.h>
#include <unistd.h>
#include "communication.h"
#include "roborts_msgs/multi_car.h"
#include "roborts_msgs/single_car.h"
#define BACKLOG 5        //完成三次握手但没有accept的队列的长度
#define CONCURRENT_MAX 8 //应用层同时可以处理的连接
#define INETADDR "192.168.3.100"  // 中转站server端的ip
//#define INETADDR "127.0.0.1"  // 中转站server端的ip
#define SERVER_PORT 2347
#define BUFFER_SIZE 1024
#define CLIENTCNT 1
#define QUIT_CMD ".quit"
int client_fds[CONCURRENT_MAX];

class TcpServer
{
public:
    float input_msg[BUFFER_SIZE] = {2.2, 2.3};
    float recv_msg[BUFFER_SIZE]; //改为float
    struct sockaddr_in server_addr;
    int bind_result;
    int server_sock_fd = 0;
    //fd_set
    fd_set server_fd_set;
    int max_fd = -1;
    struct timeval tv; //超时时间设置
    void serverSend(struct message *sendMessage) ;
    void serverRecv();
    int serverInit();
};

int TcpServer:: serverInit()
{
    //本地地址
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(SERVER_PORT);
    server_addr.sin_addr.s_addr = inet_addr(INETADDR);
    bzero(&(server_addr.sin_zero), 8);
    //创建socket
    server_sock_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (server_sock_fd == -1)
    {
        perror("socket error");
        return 1;
    }
    //绑定socket
    bind_result = bind(server_sock_fd, (struct sockaddr *)&server_addr, sizeof(server_addr));
    if (bind_result == -1)
    {
        perror("bind error");
        return 1;
    }
    //listen
    if (listen(server_sock_fd, BACKLOG) == -1)
    {
        perror("listen error");
        return 1;
    }
    while (ros::ok())
    {
        tv.tv_sec = 20;
        tv.tv_usec = 0;
        FD_ZERO(&server_fd_set);
        FD_SET(server_sock_fd, &server_fd_set);

        if (max_fd < server_sock_fd)
        {
            max_fd = server_sock_fd;
        }
        //客户端连接
        for (int i = 0; i < CONCURRENT_MAX; i++)
        {

            if (client_fds[i] != 0)
            {
                FD_SET(client_fds[i], &server_fd_set);
                if (max_fd < client_fds[i])
                {
                    max_fd = client_fds[i];
                }
            }
        }
        int ret = select(max_fd + 1, &server_fd_set, NULL, NULL, &tv);
       //  printf("inselect\n");
        if (ret < 0)
        {
            perror("select 出错\n");
            continue;
        }
        else if (ret == 0)
        {
            printf("select 超时\n");
            continue;
        }
        else
        {
            if (FD_ISSET(server_sock_fd, &server_fd_set))
            {
                //有新的连接请求
                struct sockaddr_in client_address;
                socklen_t address_len;
                int client_sock_fd = accept(server_sock_fd, (struct sockaddr *)&client_address, &address_len);
                printf("new connection client_sock_fd = %d\n", client_sock_fd);
                if (client_sock_fd > 0)
                {
                    int index = -1;
                    for (int i = 0; i < CONCURRENT_MAX; i++)
                    {
                        if (client_fds[i] == 0)
                        {
                            index = i;
                            client_fds[i] = client_sock_fd;
                            break;
                        }
                    }
                    if (index >= 0)
                    {
                        printf("新客户端(%d)加入成功 %s:%d\n", index, inet_ntoa(client_address.sin_addr), ntohs(client_address.sin_port));
                    }
                    else
                    {
                        bzero(input_msg, BUFFER_SIZE);
                        printf("客户端连接数达到最大值，新客户端加入失败 %s:%d\n", inet_ntoa(client_address.sin_addr), ntohs(client_address.sin_port));
                    }
                }
            }
            int flag = 0;
            for (flag = 0; flag < CLIENTCNT ;flag++)
            {
                
                if (client_fds[flag] == 0)
                {
                    break;
                }
            }
            if (flag == CLIENTCNT)
                break;
        }
    }
    return 1;
}
void TcpServer::serverRecv()
{
    while (1)
    {
        tv.tv_sec = 20;
        tv.tv_usec = 0;
        FD_ZERO(&server_fd_set);
        //服务器端socket
        FD_SET(server_sock_fd, &server_fd_set);
      
        if (max_fd < server_sock_fd)
        {
            max_fd = server_sock_fd;
        }
        //客户端连接
        for (int i = 0; i < CONCURRENT_MAX; i++)
        {
            //printf("client_fds[%d]=%d\n", i, client_fds[i]);
            if (client_fds[i] != 0)
            {
                FD_SET(client_fds[i], &server_fd_set);
                if (max_fd < client_fds[i])
                {
                    max_fd = client_fds[i];
                }
            }
        }
        int ret = select(max_fd + 1, &server_fd_set, NULL, NULL, &tv);
      
        if (ret < 0)
        {
            perror("select 出错\n");
            continue;
        }
        else if (ret == 0)
        {
            printf("select 超时\n");
            continue;
        }
        else
        {
            for (int i = 0; i < CONCURRENT_MAX; i++)
            {
                if (client_fds[i] != 0)
                {
                    if (FD_ISSET(client_fds[i], &server_fd_set))
                    {
                        //处理某个客户端过来的消息
                        bzero(recv_msg, BUFFER_SIZE);
                        long byte_num = recv(client_fds[i], recv_msg, BUFFER_SIZE, 0);
                        if (byte_num > 0)
                        {
                            if (byte_num > BUFFER_SIZE)
                            {
                                byte_num = BUFFER_SIZE;
                            }
                            recv_msg[byte_num] = '\0';
                        
                            int y = 0;
                            byte_num = byte_num / 4; //因为是float
                            for (y = 0; y < byte_num; y++)
                                printf("客户端(%d):%f", i, recv_msg[y]);
                            printf("\n");
                        }
                        else if (byte_num < 0)
                        {
                            printf("从客户端(%d)接受消息出错.\n", i);
                        }
                        else
                        {
                            FD_CLR(client_fds[i], &server_fd_set);
                            client_fds[i] = 0;
                            printf("客户端(%d)退出了\n", i);
                        }
                    }
                }
            }
        }
    }
}
void TcpServer::serverSend(struct message *sendMessage) //改为float
{
    for (int i = 0; i < CONCURRENT_MAX; i++)
    {
        if (client_fds[i] != 0)
        {
            printf("client_fds[%d]=%d\n", i, client_fds[i]);

            send(client_fds[i], (struct message *)sendMessage, sizeof(struct message), 0);
        }
    }
}

// int main(int argc, const char *argv[])
// {

//    TcpServer server;
//    server.serverInit();
//    printf("afterInit\n");
//        while (1)
//     {
//         server.serverSend(server.input_msg);
//       //server.serverRecv();
//          sleep(1);
//     }
//     for (int i = 0; i < CONCURRENT_MAX; i++)
//     {
//         if (client_fds[i] != 0)
//         {
//             close(client_fds[i]);
//         }
//     }
//     exit(EXIT_SUCCESS);
//     return 0;
// }

TcpServer server;


void pose_cb(const roborts_msgs::multi_car::ConstPtr& msg)
{
    struct message car_info;
    for(int i=0;i<msg->multicar.size();i++)
    {
        if(msg->multicar[i].id==1&&msg->multicar[i].color==1)
        {
            car_info.red1.stamp.data = msg->multicar[i].pose.header.stamp;
            car_info.red1.pose = msg->multicar[i].pose.pose;
        }
        if(msg->multicar[i].id==2&&msg->multicar[i].color==1)
        {
            car_info.red2.stamp.data = msg->multicar[i].pose.header.stamp;
            car_info.red2.pose = msg->multicar[i].pose.pose;
        }
        if(msg->multicar[i].id==1&&msg->multicar[i].color==2)
        {
            car_info.blue1.stamp.data = msg->multicar[i].pose.header.stamp;
            car_info.blue1.pose = msg->multicar[i].pose.pose;
        }
        if(msg->multicar[i].id==2&&msg->multicar[i].color==2)
        {
            car_info.blue2.stamp.data = msg->multicar[i].pose.header.stamp;
            car_info.blue2.pose = msg->multicar[i].pose.pose;
        }
    }
    std::cout<<"camera send"<<std::endl;    
    std::cout<<ros::Time::now()<<std::endl;    
    server.serverSend(&car_info);
}

int main(int argc,char** argv)
{
    ros::init(argc, argv, "camera_send");
    ros::NodeHandle n;
    ros::Subscriber camera_send_sub = n.subscribe<roborts_msgs::multi_car>("/camera_send",1,&pose_cb);
    server.serverInit();
    ros::spin();
}
